using UnityEngine;
using System.Collections;
using System;
// @TODO - make sure agent is actually in a empty square, if not find the nearest empty square?(very occasionally an agent will ask to find a path when in a corner square which are solid to the pathfinder)
// @TODO - static and dynamic object avoidance - just need to test it



//
//    Represents a position in the maze for the creation and following of paths
//
public class node
{
	public node parent = null;  // parent node, used to follow the origin back from the goal
	public float f_cost = 0.0f;  // cost of this node to this point
	public float h_cost = 0.0f;  // cost of this node to goal
	public float g_cost = 0.0f;  // sum of g costs
	public Vector3 point;  // real world position in unity as a vector3
	public MazeLocation maze_pos; // Position in maze
	//public int x_position;  // x east/west position in the maze
	//public int z_position;  // z north/south position in the maze
}



//
// Finds a path from one node to another node using the maze information from
// the maze class
//
public class pathplanner
{
	private ArrayList path;
	private ArrayList path_copy;
	private ArrayList open_list;
	private ArrayList closed_list;
	
	private ArrayList solids_list;
	private ArrayList static_obstacles;
	private ArrayList dynamic_obstacles;
	 
	private Maze maze;
	
	private Vector3 position;
	private Vector3 goal;
	
	private float path_length;
		
	private bool goal_found = false;
	
	private bool debug_printing = false;
	private node current; //Current opened node
	private node popped_node = null;
	
	private int nodes_opened = 0;
	private int max_open_nodes; // the number of nodes that can be opened before creating a best path
	
	private bool [] closedMap;
	private bool [] dynamicObstacleMap;

	private void print(string asdf)
	{
		//MonoBehaviour.print(asdf);	
	}	
	
	public Vector3 get_goal()
	{
		return goal;	
	}
	
	//
	//	Draws the path to goal, direct line to goal, open nodes, closed nodes and top 5 nodes
	//
	public void debug_drawing ()
	{
		//Debug.DrawLine(position, goal, Color.green);
		
		// draw_debug_lists();
		
		// if path finding fails, show top 5 in open list
		//if(!goal_found)
		//{
			//draw_top_five();
		//}
		if (((GameController)GameObject. FindWithTag("GameController").GetComponent("GameController")).getDrawAStarNodes())
		{
			update_map_colors();
		}
		
		draw_debug_path();
	}
	
	
	//
	// Draws the vectors remaining in the path the agent is following
	// Note: would be nice to draw this starting from the agents current position
	//
	private void draw_debug_path()
	{
		Vector3 current_pos = new Vector3();
		Color line_colour = Color.magenta;
		
		if(!goal_found)
		{
			line_colour = Color.green;
		}
		
		
		for(int i = 0; i < path.Count; i++)
		{
			node next_node = (node)path[i];
			Vector3 next_pos = next_node.point;
			
			// draw when current has been set
			if(i > 0)
			{
				Debug.DrawLine(current_pos, next_pos, line_colour);
				//print("current_pos: " + current_pos + " next_pos: " + next_pos);
			}
			else if(popped_node != null)
			{
				Debug.DrawLine(popped_node.point, next_pos, line_colour);
			}
			
			current_pos = next_pos;
		}
		
		
		
		// permentant path line
		/*
		for(int i = 0; i < path_copy.Count; i++)
		{
			node next_node = (node)path_copy[i];
			Vector3 next_pos = next_node.point;
			
			// draw when current has been set
			if(i > 0)
			{
				Debug.DrawLine(current_pos, next_pos, Color.blue);
				//print("current_pos: " + current_pos + " next_pos: " + next_pos);
			}
			
			current_pos = next_pos;
		}
		*/
		
	}
	
	//
	// Changes the sections of the map to colors representing 
	// which list the section is on
	//
	private void update_map_colors()
	{
		//maze.resetSquareColors();
		
		// closed list first as we really want to see the open list
		foreach (node the_node in open_list)
		{
			maze.changeSquareColor(the_node.maze_pos, Color.red);	
		}
		
		foreach (node the_node in closed_list)
		{
			maze.changeSquareColor(the_node.maze_pos, Color.yellow);	
		}
		
		
		
		
		// draw solids
		/*
		foreach (MazeLocation loc in solids_list)
		{
			maze.changeSquareColor(loc, Color.green);	
		}
		*/
		
		// draw obstacles
		if(dynamic_obstacles != null)
		{
			foreach (MazeLocation loc in dynamic_obstacles)
			{
				maze.changeSquareColor(loc, Color.cyan);	
			}
		}
		
		
		if(static_obstacles != null)
		{
			foreach (MazeLocation loc in static_obstacles)
			{
				maze.changeSquareColor(loc, Color.cyan);	
			}
		}
	}
	
	
	//
	// Draws the vectors in the open and closed list
	//
	private void draw_debug_lists()
	{
		Vector3 current_pos = new Vector3();
		
		// open
		//print("draw open list: " + open_list.Count);
		for(int i = 0; i < open_list.Count; i++)
		{
			node next_node = (node)open_list[i];
			Vector3 next_pos = next_node.point;
			
			// draw when current has been set
			if(i > 0)
			{
				//print("open current_pos: " + current_pos + " next_pos:" + next_pos);
				Debug.DrawLine(current_pos, next_pos, Color.red);
				
			}
			
			current_pos = next_pos;
		}
		
		// closed
		//print("draw closed list: " + closed_list.Count);
		for(int i = 0; i < closed_list.Count; i++)
		{
			node next_node = (node)closed_list[i];
			Vector3 next_pos = next_node.point;
			
			// draw when current has been set
			if(i > 0)
			{
				//print("closed current_pos: " + current_pos + " next_pos:" + next_pos);
				Debug.DrawLine(current_pos, next_pos, Color.blue);
				
			}
			
			current_pos = next_pos;
		}
	}
	
	
	//
	// Draws the top five node paths
	//
	private void draw_top_five()
	{
		// draw current top 5
		ArrayList colours = new ArrayList();
		colours.Add(Color.cyan);
		colours.Add(Color.blue);
		colours.Add(Color.green);
		colours.Add(Color.red);
		colours.Add(Color.yellow);
		
		
		for(int i = 0; i < 5 && open_list.Count > 0; i++)
		{
			node current_node = (node)open_list[i];
		
			while(current_node.parent != null)
			{
				node next_node = current_node.parent;
				
				// draw when current has been set
				Debug.DrawLine(current_node.point, next_node.point, (Color) colours[i]);
				
				current_node = next_node;
			}
		}
		
	}
	
	//
	//	Path Planner
	//
	//	@param AgentBehaviourScript agent - used to draw the path with agents current position as origin
	//
	public pathplanner (Vector3 position, Vector3 goal, Maze maze, ArrayList static_obstacles, ArrayList dynamic_obstacles, int max_open_nodes)
	{
		maze.resetSquareColors();

		closedMap = new bool[maze.getWidth()*maze.getHeight()];
		dynamicObstacleMap = new bool[maze.getWidth()*maze.getHeight()];
		
		path = new ArrayList();
		path_copy = new ArrayList();
		
		open_list = new ArrayList();
		closed_list = new ArrayList();
		solids_list = new ArrayList();
		
		this.position = position;
		this.goal = goal;
		
		this.maze = maze;
		this.static_obstacles = static_obstacles;
		this.dynamic_obstacles = dynamic_obstacles;
		
		this.max_open_nodes = max_open_nodes;
		
		goal_found = false;
		nodes_opened = 0;
		popped_node = null;
		
		this.goal.y = position.y;
		
		// setup first node
		node current = new node();
		current.maze_pos = maze.getMazeCoord(position);
		current.point = position;
		open_list.Insert(0, current);
		
		// Check if position and goal are the same		
		if (maze.getMazeCoord(position) == maze.getMazeCoord(goal))
		{
			print("Goal found at position");
			goal_found = true;
			create_path(current);
		}
		
		//Build map of dynamic obstacles
		if (dynamic_obstacles != null)
		{
			foreach (MazeLocation location in dynamic_obstacles)
			{
				if (location.x >= 0 && location.x < maze.getWidth() &&
				    	location.z >= 0 && location.z < maze.getHeight())
				{
					dynamicObstacleMap[location.x + maze.getWidth()*location.z] = true;	
				}
			}
		}
		
	}	
	
	// TODO : Update this documentation
	// Finds a path from one point to another and populates the path varible with an array of points to follow.
	// Returns true if a path to the goal was found
	// Returns false if a path doesn't reach the goal but is the closest.
	//
	// @param Vector3 position - agents current position
	// @param Vector3 goal - where the agent wants to be
	// @param Maze maze - maze object
	// @param ArrayList static_obstacles - Array of MazeLocation objects to path find around
	// @param ArrayList dynamic_obstacles - Array of MazeLocation objects to path find around
	//
	// @return bool 
	//
	public bool find_path(int node_iterations)
	{
		
		if (goal_found == true)
			return goal_found;
			
				//make sure goal and start are not solid
		// Remember so we can put back
		bool goal_solid = maze.getMazeSquare(maze.getMazeCoord(goal)).isSolid();	
		bool start_solid = maze.getMazeSquare(maze.getMazeCoord(position)).isSolid();	
		maze.getMazeSquare(maze.getMazeCoord(goal)).setSolid(false);	
		maze.getMazeSquare(maze.getMazeCoord(position)).setSolid(false);	
		
		int count = 0;
		int goal_x = maze.getMazeX(goal);
		int goal_z = maze.getMazeZ(goal);
		

		/* Disabled so we can find path in stages
		path.Clear();
		path_copy.Clear();
		open_list.Clear();
		closed_list.Clear();
		solids_list.Clear();
		*/
		
		//print("goal_x: " + goal_x + " goal_z: " + goal_z);
		
		//bool goal_solid = maze.getMazeSquare(maze.getMazeCoord(goal)).isSolid();
		//Debug.Log("Goal solid is: " + goal_solid);
		

				
		
		// loop while open list not empty
		while(count < node_iterations && open_list.Count > 0)
		{
			count++;
			//print("open list count: " + open_list.Count);
			
			// current = pop from top of priority list
			current = (node)open_list[0];
			open_list.RemoveAt(0);
			
			//print("current x: " + x + " current z: " + z);
			
			// if current node == goal
			if(current.maze_pos == maze.getMazeCoord(goal))
			{
				print("Goal found! Count: " + count);
				goal_found = true;
				create_path(current);
				break;
			}
			else
			{
				nodes_opened++;
				open_node(current);	
				
				// add current node to closed list and map
				closedMap[current.maze_pos.x + maze.getWidth() * current.maze_pos.z] = true;
				closed_list.Add(current);
			}
			
			if(debug_printing)
			{
				print_lists();
			}
			
		}
		
		if (!goal_found && nodes_opened > max_open_nodes)
		{
			//Create best path anyway	
			if (open_list.Count > 0)
			{
				//Debug.Log("Best open list.");
				node best = best_h_cost_node(open_list);
				create_path(best);
			}
			else
			{
				//Debug.Log("Best closed list.");
				node best = best_h_cost_node(closed_list);
				create_path(best);
			}
		}
		
		// put maze square solidness back
		maze.getMazeSquare(maze.getMazeCoord(goal)).setSolid(goal_solid);	
		maze.getMazeSquare(maze.getMazeCoord(position)).setSolid(start_solid);	
	
		
		if (nodes_opened <= max_open_nodes && !goal_found)
			return false; // not finished
		
		return true; // finished
	}
	
	public bool is_goal_found()
	{
		return goal_found;	
	}
	//
	//	Gets the next best path based on lowest h cost from a list
	//
	private node best_h_cost_node(ArrayList list)
	{
		node current = null;
		node next = null;
		int most_num = 20;	
		if (list.Count < most_num)
		{
			most_num = list.Count;	
		}
		
		for(int i = 0; i < most_num; i++)
		{
			next = (node)list[i];
			
			/*
			if(current != null && next != null)
			{
				Debug.Log("current.h_cost: " + current.h_cost + " next.h_cost: " + next.h_cost);
			}
			*/
			
			if(current == null || next.h_cost < current.h_cost)
			{
				current = next;
			}
		}
		
		//Debug.Log("current.h_cost: " + current.h_cost);
		
		return current;
	}
		
	
	
	// @TODO WHAT DOES THIS DO??
	private void open_node(node current)
	{
		// create eight points from origin
		int jump = nodes_to_jump(current.point, goal);
		
		ArrayList children_points = create_child_points(current.maze_pos, jump);
		
		// loop over current node's children
		foreach(MazeLocation point in children_points)
		{
			// check they are not in closed list
			//if(not_in_list(closed_list, point))
			if(not_closed(point))
			{
				// create new node
				node new_node = create_node(point, goal, current);
				
				// if in open list, update path to lowest cost else add to open list
				add_node(new_node);
			}
		}
	}
	
	

	
	
	//
	//  Calculates a number of nodes to jump depending on the distance a node is from the goal
	//
	//	@param Vector3 point
	//	@param Vector3 goal
	//
	//	@return int
	//
	private int nodes_to_jump(Vector3 point, Vector3 goal)
	{
		int jump = 0;
		float node_distance_goal = distance_between(point, goal);
		
		// jump 1 nodes if distance is great		
		// In most circumstances jumping one node is sufficient to reduce the search space.
		// Otherwise incresing the jumping increases the time spent to rationalise jump nodes ie ensuring 
		// line of sight without walls in the way.
		if(node_distance_goal > 500.0f)
		{
			jump = 0;
		}
			
		return jump;
	}
	
	//
	// Checks if point exists in the list
	// @param ArrayList list - array of nodes to search
	// @param Hastable point
	//
	// return boolean
	//
	private bool not_in_list(ArrayList list, MazeLocation point)
	{
		bool result = true;	
		
		foreach(node list_node in list)
		{
			if(list_node.maze_pos == point)
			{
				result = false;
				break;
			}
		}
		
		return result;
	}
	
	/* Checks if a maze point has already been opened */
	private bool not_closed(MazeLocation point)
	{
		return !closedMap[current.maze_pos.x + maze.getWidth() * current.maze_pos.z];
	}
	
	
	//
	// Create child points from a parent node position
	//
	// @param int x
	// @param int z
	//
	// @return arraylist - array of points
	//
	private ArrayList create_child_points(MazeLocation location, int jump)
	{
		
		int east =  1; // x-axis
		int west = -(1); // x-axis
		
		int north = 1; // z-axis
		int south = -(1); // z-axis
		
		
	
		ArrayList points = new ArrayList();
		
		MazeLocation [] directions = new MazeLocation[8];
		
		//TODO we don't need to create these objects every node
		
		// north
		directions[0] = new MazeLocation(east, north);
		directions[1] = new MazeLocation(0, north);
		directions[2] = new MazeLocation(west, north);
		
		
		// east - west
		directions[3] = new MazeLocation(east, 0);
		directions[4] = new MazeLocation(west, 0);
		
		
		// south
		directions[5] = new MazeLocation(east, south);
		directions[6] = new MazeLocation(0, south);
		directions[7] = new MazeLocation(west, south);
		
		
		
		for(int i = 0; i < 8; i++)
		{
			//MazeLocation child = new MazeLocation(location.x + directions[i].x, location.z + directions[i].z);
			
			MazeLocation newPoint = mazeRaytrace(location, directions[i], jump);
			if (newPoint != location)
				points.Add(newPoint);
			
		}
		
		return points;
	}
		
	// Retuns the location of hit(or max length) from a psuedo ray trace in direction
	//  if a hit is dectected - if in partial jumping mode it will return the hit point if not it will return the passed location
	private MazeLocation mazeRaytrace(MazeLocation location, MazeLocation direction, int jump)
	{
		int width = maze.getWidth();
		int height = maze.getHeight();
		MazeLocation resultLocation = new MazeLocation(location.x, location.z);	
		
		//Debug.Log("nodes to jump: " + jump);
		//Debug.Log("Start");
		//Debug.Log("    direction.x: " + direction.x + " direction.z: " + direction.z);
		//Debug.Log("    location.x: " + location.x + " location.z: " + location.z);
		
		for(int i = 0; i < (1 + jump); i++)
		{
			resultLocation.x += direction.x;
			resultLocation.z += direction.z;
			
			//Debug.Log("    Top resultLocation.x: " + resultLocation.x + " resultLocation.z: " + resultLocation.z);
			
			if (resultLocation.x > width || resultLocation.z > height)
			{
				//Debug.Log("End of maze");
				resultLocation.x -= direction.x;
				resultLocation.z -= direction.z;
				
				break;
			}
			else if(maze.getMazeSquare(resultLocation).isSolid())
			{
				//Debug.Log("Solid");
				// show squares the agent thinks is solid
				solids_list.Add(resultLocation);
				
				resultLocation.x -= direction.x;
				resultLocation.z -= direction.z;
				
				break;
			}
		}
		
		
		// Dynamic and static obstacle avoidance
		if(static_obstacles != null)
		{
			// loop relocation != location and relocation in array
			while(resultLocation != location && location_in_list(resultLocation, static_obstacles))
			{
				resultLocation.x -= direction.x;
				resultLocation.z -= direction.z;
			}
		}
		
		/* Dynamic obsticales changed to just add a lot of cost rather than completly block
		if(dynamic_obstacles != null)
		{
			// loop relocation != location and relocation in array
			while(resultLocation != location && location_in_list(resultLocation, dynamic_obstacles))
			{
				resultLocation.x -= direction.x;
				resultLocation.z -= direction.z;
			}
		}
		*/
		

		
		//Debug.Log("    Bottom resultLocation.x: " + resultLocation.x + " resultLocation.z: " + resultLocation.z);
			
		//Debug.Log("End");
		
		return resultLocation;	
	}
	
	
	
	//
	//	Returns true if location is found in the list
	//
	//	@param MazeLocation location - location to test
	//	@param ArrayList list - Array of locations to match
	//
	//	@return bool
	//
	private bool location_in_list(MazeLocation location, ArrayList list)
	{
		bool result = false;
		
		foreach (MazeLocation loc in list)
		{
			if(location == loc)
			{
				result = true;
				break;
			}
		}
		
		return result;
	}

	
	//
	//	Adds the new node to the open list according the its f cost
	//
	//	@param node new_new
	//
	//	@return void
	//
	private void add_node(node new_node)
	{
		node current = null;
		bool node_added = false;
		
		// add new node if the open list is empty
		if(open_list.Count == 0)
		{
			open_list.Add(new_node);
		}
		else
		{
			// add the node to the open list in order of ascending f cost
			for(int i = 0; i < open_list.Count; i++)
			{
				//print("add_node i: " + i);
				
				current = (node)open_list[i];
				
				//print("new_node.f_cost: " + new_node.f_cost + " current.f_cost: " + current.f_cost);
				
				
				// if matching nodes, update list
				if(new_node.maze_pos == current.maze_pos)
				{
					// update list if new node cost is less
					if(new_node.g_cost < current.g_cost)
					{
						open_list[i] = new_node;		
					}
					
					node_added = true;
					break;
				}
				// add node in cost order
				else if(new_node.f_cost < current.f_cost)
				{
					//print("inset to open list");
					
					//print("open_list before: " + open_list.Count);
					open_list.Insert(i, new_node);
					//print("open_list after: " + open_list.Count);
					
					node_added = true;
					break;
				}
				
			}

			
			// if it is the worst cost add to the end
			if(!node_added)
			{
				//print("add to open list");
				open_list.Add(new_node);
			}	
		}
		
	}
	
	
	//
	//	Returns true if a node collision occurs in the open list, 
	//	the list is updated according the lowest f cost of the matching nodes
	//
	//	@DEPRECATED - use add node only as it does the check
	//
	//	@param node new_new
	//
	//	@return bool
	//
	private bool has_updated_open_list(node new_node)
	{
		node current = null;
		bool result = false;
		
		// find same node
		for(int i = 0; i < open_list.Count; i++)
		{
			current = (node)open_list[i];
			
			// check matching nodes
			if(new_node.maze_pos == current.maze_pos)
			{
				// update list if new node cost is less
				if(new_node.f_cost < current.f_cost)
				{
					open_list[i] = new_node;		
				}
				
				result = true;
				
				break;
			}
		}
		
		return result;
	}
	
	
	
	//
	// Creates a new node
	// @param Vector3 point - current position of node
	// @param Vector3 goal
	// @param node parent - parent node
	//
	// @return node 
	//
	private node create_node(MazeLocation new_node_pos, Vector3 goal, node parent)
	{
		node new_node = new node();	
		
		new_node.maze_pos = new_node_pos;
		new_node.point = maze.getMazePosition(new_node_pos);
		new_node.parent = parent;
		
		float g_cost = distance_between(parent.point, new_node.point) + getAddedDynamicObstacleCost(new_node.maze_pos);
		float h_cost = distance_between(goal, new_node.point);
		
		//new_node.g_cost += g_cost;// stupid bug
		new_node.g_cost = parent.g_cost + g_cost; // proper cost
		new_node.h_cost = h_cost;
		new_node.f_cost = new_node.g_cost + h_cost;
		
		//Debug.Log("g_cost: " + g_cost + " h_cost: " + h_cost + " parent.f_cost: " + parent.f_cost);
		
		return new_node;
	}
	
	private float getAddedDynamicObstacleCost(MazeLocation location)
	{
		float addedCost = 0.0f;
		float addedCostPerObstacle = 3.0f * maze.getCellSize();
			
		for (int j = location.z - 2; j <= location.z + 2; j++)
		{
			for (int i = location.x - 2; i <= location.x + 2; i++)
			{
			
				if (i >= 0 && i < maze.getWidth() &&
			    	j >= 0 && j < maze.getHeight())
				{
					if (dynamicObstacleMap[i + j*maze.getWidth()] == true)
						addedCost += addedCostPerObstacle;	
				}
			}
		}
		
		return addedCost;
	}
	
	//
	//  Creates and sets a new path from the given node
	//
	//  @param node goal - node to create a path from
	//	@return void
	private void create_path(node goal)
	{
		node current = goal;
		node next = null;
		this.path.Insert(0, current);
		
		path_length = goal.g_cost;
		
		// wind the search nodes backwards to create a path
		while(current.parent != null)
		{
			next = current.parent;
			//path_length += Vector3.Distance(current.point, next.point);	
					
			this.path.Insert(0, next);
			current = next;
		}
		
		path_copy = (ArrayList) path.Clone();
	}
	
	
	//
	// Returns the Euclidean distance between two points
	//
	private float distance_between(Vector3 point1, Vector3 point2)
	{
		float result = Vector3.Distance(point1, point2);
		
		return result;
	}
	
	
	//
	//  Agent can set the path 
	//
	public void set_path(ArrayList path)
	{
		this.path = path;
	}
	
	
	//
	//  Get the path 
	//
	public ArrayList get_path()
	{
		return this.path;
	}
	
	public void setup_line_renderer(LineRenderer renderer)
	{
		if (!is_goal_found()) //don't touch if we havn't completed
			return;
			
		renderer.SetVertexCount(path.Count);
		int i = 0;
		foreach( node currNode in path )
		{
			renderer.SetPosition(i, currNode.point);
			i++;
		}
	}
	
	//
	// Returns the next vector3 to follow
	// @return Vector3 - point to follow
	//
	public Vector3 next_path_vector()
	{
		node next = null;
		Vector3 point = new Vector3();
			
		if(has_next_point())
		{
			next = (node)path[0];
			path.RemoveAt(0);
			point = next.point;
			popped_node = next;
		}
		
		return point;
	}
	
	
	//
	// Returns the next node to follow
	// @return node - node to follow
	//
	public node next_path_node()
	{
		node next = null;
			
		if(has_next_point())
		{
			next = (node)path[0];
			path.RemoveAt(0);
			popped_node = next;
		}
		
		return next;
	}
	
	public float get_path_length()
	{
		return path_length;	
	}
	
	//
	// Returns true if the path has more points to follow.
	//
	// AGENTS MUST USE THIS FOR PATH FOLLOWING AND BEFORE CALLING next_point().
	//
	// @return bool
	//
	public bool has_next_point()
	{
		bool result = false;
		
		if(path.Count > 0)
		{
			result = true;
		}
		
		return result;
	}
	
	
	//
	//	Prints open and closed lists
	//
	private void print_lists()
	{
		// print open and closed lists
		string open = "";
		string closed = "";
		
		for(int i = 0; i < open_list.Count; i++)
		{
			node open_node = (node)open_list[i];
			open = open + "(" + open_node.maze_pos.x + "," + open_node.maze_pos.z + ") ";
		}
		print("open: " + open);
			
		for(int i = 0; i < closed_list.Count; i++)
		{
			node closed_node = (node)closed_list[i];
			closed = closed + "(" + closed_node.maze_pos.x + "," + closed_node.maze_pos.z + ") ";
		}
		print("closed: " + closed);
	}
}

